package com.tmrnavi.sensormeasureservice;

import android.app.Service;
import android.content.Context;
import android.content.Intent;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.os.Handler;
import android.os.IBinder;
import android.os.Message;

import java.util.Timer;
import java.util.TimerTask;

public class SensorService extends Service {

    public SensorManager sensorManager;
    public Sensor sensor_gyro, sensor_acc, sensor_mag, sensor_att, sensor_pre;
    public static SensorEventListener sensorEventListener;

    // 传感器数据初始化
    private float[] wib = new float[]{0.0f, 0.0f, 0.0f}, avg_wib = new float[3];    // 角速度与平滑后角速度
    private float[] fsf = new float[]{0.0f, 0.0f, 0.0f}, avg_fsf = new float[3];    // 加速度与平滑后加速度
    private float[] mag = new float[]{0.0f, 0.0f, 0.0f}, avg_mag = new float[3];    // 磁场与平滑后磁场
    private float pre = 0, avg_pre = 0, presAlt = 0.0f;                             // 气压、平滑后气压与气压高度
    private float[] qnb = new float[]{0.0f, 0.0f, 0.0f, 0.0f}, att = new float[3];     // quaternion
    private float[] fsf0 = new float[3];                // 用于降噪
    private double[] avppx = new double[10+16+16+1];
    private int[] CNT = new int[]{0, 0, 0, 0};          // 计数器
    private boolean IMUUPDATE = false;                  // IMU 数据更新标志
    private final PSINSDataStore psinsDataStore = new PSINSDataStore(this);

    private TimerTask IMUdataTask;
    private static Timer naviTimer;             // 定时器
    private static int k = 0;                   // 采样计数
    private final int IDLESTATE = 400;          // 空闲状态标志
    private final int ALIGNSTATE = 100;         // 对准状态标志
    private final int CPNAVRDY   = 200;         // 组合导航准备标志
    private final int CPNAVSTATE = 201;         // 组合导航状态标志
    private static int navState;                // 导航状态变量
    public static long t_stp, t0_stp;                           // 导航更新时间戳
    public static float t = 0.0f;                               // 导航时长

    private int   INSCNT = 0;                           // INS计数
    private final float PI = 3.141592653f;
    private final float DEG = PI / 180;
    private final float SEC = DEG/3600;
    private final float MG = (float) (9.8/1000.0);
    private LPFilter lpFilterW, lpFilterF, lpFilterM;

    private Handler msgHandler;
    private float[] data;
    private boolean isSave;

    public SensorService() {
    }

    @Override
    public int onStartCommand(Intent intent, int flags, int startId) {
        isSave = intent.getBooleanExtra("savedata", false);
        sensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
        sensor_gyro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
        sensor_acc  = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        sensor_mag  = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        sensor_att  = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
        sensor_pre  = sensorManager.getDefaultSensor(Sensor.TYPE_PRESSURE);

        if (isSave) { psinsDataStore.open();}
        lpFilterW = new LPFilter();         // 三个低通滤波器
        lpFilterF = new LPFilter();
        lpFilterM = new LPFilter();
        data = new float[10];

        msgHandler = MainActivity.msgHandler;
        naviTimer = new Timer();            // Navigation Timer Initialization

        // 接收传感器数据
        sensorEventListener = new SensorEventListener() {
            @Override
            public void onSensorChanged(SensorEvent event) {
                switch (event.sensor.getType()) {
                    case Sensor.TYPE_GYROSCOPE:         // 陀螺输出角速度
                        wib[0] += event.values[0];
                        wib[1] += event.values[1];
                        wib[2] += event.values[2];
                        CNT[0]++;
                        break;
                    case Sensor.TYPE_ACCELEROMETER:     // 加计输出比力
                        fsf[0] += event.values[0];
                        fsf[1] += event.values[1];
                        fsf[2] += event.values[2];
                        CNT[1]++;
                        break;
                    case Sensor.TYPE_MAGNETIC_FIELD:    // 磁场强度向量
                        mag[0] += event.values[0];
                        mag[1] += event.values[1];
                        mag[2] += event.values[2];
                        CNT[2]++;
                        break;
                    case Sensor.TYPE_ROTATION_VECTOR:   // 姿态四元数（Android计算姿态结果）
                        qnb[0] = event.values[3];
                        qnb[1] = event.values[0];
                        qnb[2] = event.values[1];
                        qnb[3] = event.values[2];
                        break;
                    case Sensor.TYPE_PRESSURE:          // 气压
                        pre += event.values[0];
                        CNT[3]++;
                        break;
                }
            }

            @Override
            public void onAccuracyChanged(Sensor sensor, int accuracy) {
            }
        };

        // 注册传感器
        sensorManager.registerListener(sensorEventListener, sensor_gyro, SensorManager.SENSOR_DELAY_FASTEST);
        sensorManager.registerListener(sensorEventListener, sensor_acc, SensorManager.SENSOR_DELAY_FASTEST);
        sensorManager.registerListener(sensorEventListener, sensor_mag, SensorManager.SENSOR_DELAY_FASTEST);
        sensorManager.registerListener(sensorEventListener, sensor_att, SensorManager.SENSOR_DELAY_FASTEST);
        sensorManager.registerListener(sensorEventListener, sensor_pre, SensorManager.SENSOR_DELAY_FASTEST);

        // 定时器任务用于定时输出
        IMUdataTask = new TimerTask() {
            @Override
            public void run() {
                t_stp = System.currentTimeMillis();
                t_stp = t_stp - t0_stp;
                t = t_stp / 1000.0f;
                if (CNT[0] != 0 && CNT[1] != 0 && CNT[2] != 0) {    // 采样周期内的传感器数据取平均
                    for (int i = 0; i < 3; i++) {                   // 气压计的频率太低，所以不放在一起
                        avg_wib[i] = wib[i] / CNT[0] / DEG;
                        wib[i] = 0;
                        avg_fsf[i] = fsf[i] / CNT[1];
                        fsf[i] = 0;
                        avg_mag[i] = mag[i] / CNT[2];
                        mag[i] = 0;
                    }
//                    Log.v("snesor_cnt", CNT[0]+"\t"+CNT[1]+"\t"+CNT[2]+"\t"+CNT[3]);
                    if (CNT[3]!=0) {                        // 目前仅读取数据
                        avg_pre = pre / CNT[3];
                        pre = 0;
                        /* ****** 计算气压高度 ******* */
                        presAlt = (float) (44300*(1- Math.pow((avg_pre/1013.25),0.1902587519025)));
                        /* ************************** */
                    }
                    CNT[0] = 0;
                    CNT[1] = 0;
                    CNT[2] = 0;
                    CNT[3] = 0;
                    IMUUPDATE = true;
                }

                if (IMUUPDATE) {
                    /* ********* 排除加速度异常值 ***************** */
                    if ((fsf0[0] == 0 || fsf0[1] == 0 || fsf0[2] == 0) && INSCNT <= 100) {             // 排除加速度异常值
                        fsf0 = avg_fsf;
                    } else {
                        for (int i = 0; i < 3; i++) {
                            if (Math.abs(fsf0[i] - avg_fsf[i]) > 2) {
                                avg_fsf[i] = fsf0[i];
                            } else {
                                fsf0[i] = avg_fsf[i];
                            }
                        }
                    }
                    /* *********************************************** */

                    avg_wib = lpFilterW.update(avg_wib);        // 角速度低通滤波
                    avg_fsf = lpFilterF.update(avg_fsf);        // 加速度低通滤波
                    avg_mag = lpFilterM.update(avg_mag);        // 磁力计低通滤波
                    System.arraycopy(avg_wib, 0, data, 0,3);
                    System.arraycopy(avg_fsf, 0, data, 3,3);
                    System.arraycopy(avg_mag, 0, data, 6,3);
                    data[9] = t;

                    INSCNT++;       // 导航计数+1
                    IMUUPDATE = false;  // 清除IMU更新标志

                    /*===========  解算  ==========*/
                    if (INSCNT >= 200) {        // 等待滤波结果稳定再解算(2s) Android传感器竟然也有启动时间
                        k++;
                        if (k >= 19) {          // 以较慢的频率刷新传感器数据
                            Message msg1 = msgHandler.obtainMessage();
                            msg1.arg1 = MainActivity.DISPMSG_SENSOR;
                            Bundle bundle = new Bundle();
                            bundle.putFloatArray("data", data);
                            msg1.setData(bundle);
                            msgHandler.sendMessage(msg1);
                            k = 0;
                        }
                        if (isSave) { psinsDataStore.writedata(avg_wib, avg_fsf, avg_mag, att, qnb, t);}
                    }
                }
            }
        };
        naviTimer.schedule(IMUdataTask, 0,10);     // Start IMU data sampling

        return super.onStartCommand(intent,flags,startId);
    }

    @Override
    public void onDestroy() {
        IMUdataTask.cancel();
        if (isSave) { psinsDataStore.close();}
    }

    @Override
    public IBinder onBind(Intent intent) {
        // TODO: Return the communication channel to the service.
        throw new UnsupportedOperationException("Not yet implemented");
    }
}